% Maximum orbit radius transfer problem solved with a shooting method.
global T mdot
T = 0.1405;
mdot = 0.07489;
tf = 3.3155; % final time
xi = [1,0,1]'; % initial constraint
%%% Optimization options
optim = optimset('fsolve');
optim.Display = 'off';
optim.Algorithm = 'levenberg-marquardt';

%%% Start guess
lambda0 = [-1; -1; -1];
m = terminalStateCondition(lambda0, xi, 0, tf, 0);
%%% Solve the problem
maxIter = 20;
maxConditionError = 1e-3;
for k = 1:maxIter
  [t,s] = ode23(@maxRadiusOrbitTransferEqAndAdjointEq, [0 tf], [xi;lambda0]);
  %plotResult(t, s, [], false); % uncomment if you wanna see the current solution!
  if norm( terminalStateCondition(lambda0, xi, 0, tf, 0) ) < maxConditionError
    break;
  end
  m = (1-k/maxIter).*m; % push m towards zero
  lambda0 = fsolve(@terminalStateCondition, lambda0, optim, xi, 0, tf, m); % finds a lambda0 such that terminalStateCondition(lambda0) = m
end
if norm( terminalStateCondition(lambda0, xi, 0, tf, 0) ) > maxConditionError
  disp('Warning! Terminal constraints not sufficiently small!'); 
end
finalRadius = s(end,1);
fprintf('Final radius: %f \n', finalRadius)
plotResult(t, s);